#ifndef IMU_API_H
#define IMU_API_H

#include "baseTask.h"
#include "../tool/configManage.h"
#include "../device/imuDevice.h"
#include <iostream>
#include <thread>
#include <unistd.h> // 引入 usleep()
#include "../include/tool/log.h"

//FDlink candata
#define FRAME_HEAD  0xfc
#define FRAME_END   0xfd
#define TYPE_IMU    0x40
#define TYPE_AHRS   0x41
#define TYPE_INSGPS 0x42
#define TYPE_GROUND 0xf0
#define IMU_LEN     0x38   //56+8  8组数据
#define AHRS_LEN    0x30   //48+8  7组数据
#define INSGPS_LEN  0x42 //72+8  10组数据
#define IMU_CAN 9
#define AHRS_CAN 8
#define INSGPS_CAN 11


typedef struct IMUData_Packet_t{
    float gyroscope_x;          //unit: rad/s
    float gyroscope_y;          //unit: rad/s
    float gyroscope_z;          //unit: rad/s
    float accelerometer_x;      //m/s^2
    float accelerometer_y;      //m/s^2
    float accelerometer_z;      //m/s^2
    float magnetometer_x;       //mG
    float magnetometer_y;       //mG
    float magnetometer_z;       //mG
    float imu_temperature;      //C
    float Pressure;             //Pa
    float pressure_temperature; //C
    uint32_t Timestamp;         //us
} IMUData_Packet_t;

typedef struct AHRSData_Packet_t {
	float RollSpeed;   //unit: rad/s
	float PitchSpeed;  //unit: rad/s
	float HeadingSpeed;//unit: rad/s
	float Roll;        //unit: rad
	float Pitch;       //unit: rad
	float Heading;     //unit: rad
	float Qw;//w          //Quaternion
	float Qx;//x
	float Qy;//y
	float Qz;//z
	uint32_t Timestamp; //unit: us
}AHRSData_Packet_t;


#define RECV_IMU 1
#define RECV_AHRS 2

class C_imuApi : public C_baseTask
{
private:
    /* data */
    std::string task_name;
    int task_period;
    std::string task_topic;

    C_imuDevice *p_imuDevice;
    std::vector<uint8_t> m_imuStrBuffer;
    IMUData_Packet_t IMUData_Packet;
    AHRSData_Packet_t AHRSData_Packet;
    /* function */
    
public:

    C_imuApi(/* args */);
    ~C_imuApi();

    int32_t initTask(CONFIG_IMU &config_imu);
    void runLoopTask(void);

    int32_t getImuDeviceData(std::vector<uint8_t> &dataBuf); // 接收IMU的串口数据
    int32_t parseImuData(std::vector<uint8_t> &dataBuf); //解析接收到的IMU的设备数据    
    int32_t protocolDataCheck(std::vector<uint8_t> &dataBuf ); // 协议数据校验
};

#endif
